#include "CPhysicsMain.h"
#include "../Objects/CObjects.h"
#include "../src/ErrorOutputs.h"

CPhysicsMain::CPhysicsMain()
{
}

CPhysicsMain::~CPhysicsMain()
{
}

void CPhysicsMain::setupPhysicsWorld()
{
    m_broadphase = new btDbvtBroadphase();
    m_collisionConfiguration = new btDefaultCollisionConfiguration();
    m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
    m_solver = new btSequentialImpulseConstraintSolver;
    m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
    m_dynamicsWorld->setGravity(btVector3(0,-10,0));
}

void CPhysicsMain::destroyPhysicsWorld()
{
    delete m_dynamicsWorld;
    delete m_solver;
    delete m_dispatcher;
    delete m_collisionConfiguration;
    delete m_broadphase;
}

void CPhysicsMain::startPhysicsCycle()
{
    ;
}

void CPhysicsMain::advanceOneStep(double timeSinceLastCallInSeconds)
{
    double fixedTimeStep = 1.0 / 60.0;
    int subSteps = 1;

    subSteps = static_cast<int>(fixedTimeStep / timeSinceLastCallInSeconds );
    subSteps = (subSteps >= 1) ? subSteps : 1;

    m_dynamicsWorld->stepSimulation(timeSinceLastCallInSeconds,subSteps,fixedTimeStep);
//    std::cout << "time: " << timeSinceLastCallInSeconds << "substeps: " << subSteps << "fixedTimeStep: " << fixedTimeStep << std::endl;
//    std::cout << "prod: " << fixedTimeStep * subSteps << std::endl;
//    btTransform trans;
//    std::list<CBaseObject*>::iterator itPlayer = m_objectList->begin();
//    itPlayer++;
//    static_cast<CBaseObject*>(*itPlayer)->getWorldTransform(trans);
//    std::cout << "sphere height: " << trans.getOrigin().getY() << std::endl;
}

void CPhysicsMain::addObject(BaseObject* object)
{
    if(m_dynamicsWorld == NULL)
    {
        writeExceptionOutput("Tried to add an object without dynamics world");
    }

    if(object->getRigidBody() == NULL)
    {
        writeExceptionOutput("Object is not yet ready to add. Call prepareObject() first");
    }
    m_dynamicsWorld->addRigidBody(object->getRigidBody());

}

void CPhysicsMain::removeObject(BaseObject* object)
{
    if(m_dynamicsWorld == NULL)
    {
        writeExceptionOutput("Tried to remove an object without dynamics world");
    }

    if(object->getRigidBody() == NULL)
    {
        writeExceptionOutput("No object to remove");
    }

    m_dynamicsWorld->removeRigidBody(object->getRigidBody());
}

void CPhysicsMain::prepareObject(BaseObject *object)
{
    switch(object->getType())
    {
    case BaseObject::TYPE_NONE:
        {
        return;
        }
    case BaseObject::TYPE_FLOOR:
        {
        object->setCollisionShape(new btStaticPlaneShape(btVector3(0,1,0),0));
        btRigidBody::btRigidBodyConstructionInfo groundRigidBodyCI(0,object, object->getCollisionShape(),btVector3(0,0,0));
        object->setRigidBody(new btRigidBody(groundRigidBodyCI));
        break;
        }
    case BaseObject::TYPE_SPHERE:
        {
        btScalar radius = dynamic_cast<SphereObject*>(object)->radius();
        object->setCollisionShape(new btSphereShape(radius));
        btScalar mass = dynamic_cast<SphereObject*>(object)->mass();
        btVector3 fallInertia(0,0,0);
        object->getCollisionShape()->calculateLocalInertia(mass,fallInertia);
        btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass,object,object->getCollisionShape(),fallInertia);
        object->setRigidBody(new btRigidBody(fallRigidBodyCI));
        break;
        }
    default:
        {
        return;
        }
    }

    if(object->isControlable())
    {
        object->getRigidBody()->setCollisionFlags( object->getRigidBody()->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
        object->getRigidBody()->setActivationState(DISABLE_DEACTIVATION);
    }
    object->isPhysicsRelevant(true);

}

